#include "ethercat_motor_control.h"
#include "ethercat.h"

#define POSITION_INDEX 0x6064  
#define POSITION_SUBINDEX 0x00 

#define VELOCITY_INDEX 0x606C  
#define VELOCITY_SUBINDEX 0x00

#define TORQUE_INDEX 0x6077    
#define TORQUE_SUBINDEX 0x00  

Motor_or_IO devices[128];

int slaveid;
int target_position;
int speed;

int32_t position = 0;
int32_t velocity = 0;
int16_t torque = 0;
uint8_t mode = 0;
uint16_t statusWord = 0;
uint16_t controlWord = 0;

char netcard[64]; // Default network interface
int slave_count = 0;   // Number of motors





void delay_ms(int milliseconds)
{
    Sleep(milliseconds); // Use Windows Sleep function
}

void enable_motor(int slave_position)
{

    int size = sizeof(statusWord);
    ec_SDOread(slave_position, 0x6041, 0x00, FALSE, &size, &statusWord, EC_TIMEOUTRXM);
    printf("statusWord: %d \n",statusWord);

    size = sizeof(mode);
    ec_SDOread(slave_position, 0x6061, 0x00, FALSE, &size, &mode, EC_TIMEOUTRXM);
    printf("mode: %d \n",mode);   

    uint8_t mode_w = 1;
    ec_SDOwrite(slave_position, 0x6060, 0x00, FALSE, sizeof(mode_w), &mode_w, EC_TIMEOUTRXM);
    delay_ms(150);

    size = sizeof(mode);
    ec_SDOread(slave_position, 0x6061, 0x00, FALSE, &size, &mode, EC_TIMEOUTRXM);
    printf("mode: %d \n",mode);  

    controlWord = 0x0006; // Prepare enable
    ec_SDOwrite(slave_position, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
    delay_ms(150);

    controlWord = 0x0007; // Enable motor
    ec_SDOwrite(slave_position, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
    delay_ms(150);

    controlWord = 0x000F; // Start motion
    ec_SDOwrite(slave_position, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
    delay_ms(150);

    ec_SDOread(slave_position, 0x6041, 0x00, FALSE, &size, &statusWord, EC_TIMEOUTRXM);
    printf("statusWord: %d \n",statusWord);

    printf("motor on slave %d , enabled \n",slave_position);
}

int read_all_motors()
{
    for (int i = 0; i < slave_count; i++)
    {
        if(devices->type == 1)//motor 
        {

            
            int size = sizeof(statusWord);
            ec_SDOread(devices[i].ethercatid, 0x6041, 0x00, FALSE, &size, &statusWord, EC_TIMEOUTRXM);
            devices[i].status = statusWord;
            
            int size1 = sizeof(position);
            ec_SDOread(devices[i].ethercatid, POSITION_INDEX, POSITION_SUBINDEX, FALSE, &size1, &position, EC_TIMEOUTRXM);
            devices[i].current_position = position;


            int size2 = sizeof(velocity);
            ec_SDOread(devices[i].ethercatid, VELOCITY_INDEX, VELOCITY_SUBINDEX, FALSE, &size2, &velocity, EC_TIMEOUTRXM);
            devices[i].current_speed = velocity;
            
            int size3 = sizeof(torque);
            ec_SDOread(devices[i].ethercatid, TORQUE_INDEX, TORQUE_SUBINDEX, FALSE, &size3, &torque, EC_TIMEOUTRXM);
            devices[i].current_torque = torque;            
        }
    }
    return 1;
}

int enable_all_motors()
{
    printf("slave count: %d \n",slave_count);
    for (int i = 0; i < slave_count; i++)
    {
        if(devices->type == 1)//motor 
        {

            devices[i].target_position = 0;
            devices[i].velocity = 0;
            devices[i].acceleration = 0;
            devices[i].deceleration = 0;
            devices[i].current_position = 0;
            devices[i].current_speed = 0;
            devices[i].current_torque = 0;
            devices[i].is_running = 0; // Initially stopped

            enable_motor(devices[i].ethercatid);

            // Check if the motor is enabled
            uint16_t statusWord = 0;
            int size = sizeof(statusWord);
            ec_SDOread(devices[i].ethercatid, 0x6041, 0x00, FALSE, &size, &statusWord, EC_TIMEOUTRXM);

            if (!(statusWord & 0x0027))
            { // Check if the motor is in "Operation Enabled" state
                char error_message[100];
                snprintf(error_message, sizeof(error_message), "Motor %d failed to enable", devices[i].ethercatid);
                // send_error(error_message);
                return -1;
            }
            else
            {
                printf("slave %i as motor enabled \n",i);
            }
        }
    }
    return 0;
}

int SV630N(int ec_id, int32_t target_position, int32_t profile_velocity)
{
    controlWord = 0xf;
    ec_SDOwrite(ec_id, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
    // delay_ms(50);

    // Set profile velocity
    ec_SDOwrite(ec_id, 0x6081, 0x00, FALSE, sizeof(profile_velocity), &profile_velocity, EC_TIMEOUTRXM);
    // delay_ms(50);
    printf("profile_velocity: %d \n",profile_velocity);

    // Set target position
    ec_SDOwrite(ec_id, 0x607A, 0x00, FALSE, sizeof(target_position), &target_position, EC_TIMEOUTRXM);
    // delay_ms(50);

    controlWord = 0x1f;
    ec_SDOwrite(ec_id, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
    // delay_ms(50);

    return 0;
}

int doStop(int ec_id){
    controlWord = 0x7f;
    ec_SDOwrite(ec_id, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);

    return 0;
}

int doStop1(int ec_id){
    controlWord = 0x6;
    ec_SDOwrite(ec_id, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);

    return 0;
}

int doStop2(int ec_id){
    controlWord = 0x7;
    ec_SDOwrite(ec_id, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);

    return 0;
}




void read_config(const char *config_file)
{
    json_t *root;
    json_error_t error;
    root = json_load_file(config_file, 0, &error);

    if (!root)
    {
        fprintf(stderr, "error: on line %d: %s\n", error.line, error.text);
        return;
    }


    json_t *netcard_json = json_object_get(root, "netcard");


    if (json_is_string(netcard_json))
    {
        strncpy(netcard, json_string_value(netcard_json), sizeof(netcard) - 1);
    }

    // Read motors array
    json_t *motors_json = json_object_get(root, "motors");
    if (!json_is_array(motors_json))
    {
        fprintf(stderr, "error: 'motors' field is not an array\n");
        json_decref(root);
        return;
    }

    size_t index;
    json_t *value;
    slave_count = 0;
    json_array_foreach(motors_json, index, value)
    {

        json_t *type_json = json_object_get(value, "type");
        json_t *ethercatid_json = json_object_get(value, "ethercatid");

        if (!json_is_string(type_json) || !json_is_integer(ethercatid_json))
        {
            fprintf(stderr, "error: invalid motor configuration at index %zu\n", index);
            continue;
        }

        devices[slave_count].type = strcmp(json_string_value(type_json), "servo") == 0 ? 1 : 0;
        // devices[slave_count].type = strcmp(json_string_value(type_json), "IO") == 0 ? 2 : 0;
        devices[slave_count].ethercatid = (int)(json_integer_value(ethercatid_json));
        devices[slave_count].is_running = 0; // Initialized as not running
        slave_count++;
    }

    json_decref(root);
}

int init_soem(int argc, char *argv[])
{
    if (argc < 2)
    {
        printf("Usage: %s <config.json>\n", argv[0]);
        return -1;
    }

    read_config(argv[1]);  
    // Initialize EtherCAT master
    if (ec_init(netcard))
    {
        printf("EtherCAT initialization succeeded.\n");
    }
    else
    {
        printf("EtherCAT initialization failed.\n");
        return -1;
    }

    // Configure slaves
    if (ec_config_init(FALSE) > 0)
    {
        printf("EtherCAT configuration succeeded.\n");
    }
    else
    {
        printf("EtherCAT configuration failed.\n");
        ec_close();
        return -1;
    }

    // Initialize motors
    if (enable_all_motors() != 0)
    {
        printf("enable_all_motors failed.\n");
        ec_close();
        return -1;
    }

    printf("soem inited.\n");
    

    return 0;
}